﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows.Forms;

namespace Control
{
    public class cls_Machine
    {
        public static bool isDeviceOpened = false;
        private static cls_Machine _instance;

        public static cls_Machine Instance()
        {
            // Uses lazy initialization.
            // Note: this is not thread safe.
            if (_instance == null)
            {
                _instance = new cls_Machine(9,10);
            }

            return _instance;
        }

        public cls_Machine(byte first,byte second)
        {
            Global.gFirstBoard = first;
            Global.gSecondBoard = second;
            Open1stBoard();
            Open2ndBoard();
            isDeviceOpened = true;
        }
        ~ cls_Machine()
        {
            //1st
            int errCode = P1240.P1240MotDevClose(Global.gFirstBoard);
            //2nd
            errCode = P1240.P1240MotDevClose(Global.gSecondBoard);
        }

        private void Open1stBoard()
        {
            int errCode = 0;
            //1st
            errCode = P1240.P1240MotDevOpen(Global.gFirstBoard);
            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return;
            }
        }

        private void Open2ndBoard()
        {
            int errCode = 0;
            //2nd
            errCode = P1240.P1240MotDevOpen(Global.gSecondBoard);
            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return;
            }
        }
        public void SetupStep()
        {
            Setup1stStep();
            Setup2ndStep();
        }
        private void Setup1stStep()
        {
            uint dwSV = 1000;   //Start Velocity
            uint dwDV = 8000;   //Drive Speed
            uint dwAC = 1000000;//600000;  //Acceleration Speed
            uint dwAK = 62500000;//10000000;  //Acceleration Rate
            uint dwMDV = 8000; //Maximum Drive Speed
            long errCode = P1240.P1240MotAxisParaSet(
                 Global.gFirstBoard,         // Board ID
                 (byte)P1240.XZ_Axis,             // Interpolation axes
                 (byte)P1240.TCurveAcceleration,   // s Curve Acceleration
                 dwSV,                 // SV
                 dwDV,                 // DV
                 dwMDV,                // Max DV
                 dwAC,                 // AC
                 dwAK);                // AK
            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return;
            }

        }
        private void Setup2ndStep()
        {
            uint dwSV = 1000;   //Start Velocity
            uint dwDV = 8000;   //Drive Speed
            uint dwAC = 1000000;  //Acceleration Speed
            uint dwAK = 62500000;  //Acceleration Rate
            uint dwMDV = 8000; //Maximum Drive Speed

            long errCode = P1240.P1240MotAxisParaSet(
                 Global.gSecondBoard,         // Board ID
                 (byte)P1240.XYZ_Axis,             // Interpolation axes
                 (byte)P1240.TCurveAcceleration,   // s Curve Acceleration
                 dwSV,                 // SV
                 dwDV,                 // DV
                 dwMDV,                // Max DV
                 dwAC,                 // AC
                 dwAK);                // AK
            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return;
            }
              
        }

        public void OpenServoSource()
        {
            //Set Card 1 truc X1
            int retMsg = P1240.P1240MotDO(
                    Global.gFirstBoard,
                    (byte)P1240.X_Axis,
                    9);
        }

        public void ServoOn()
        {
            ServoOn1stCard();
            ServoOn2ndCard();
        }


        private void ServoOn1stCard()
        {
            int retMsg;
            //Set emg, stroke, servoOn 

            //Setting for 1st
            //Set truc X1(Z thuc)
            retMsg = P1240.P1240MotDO(
                   Global.gFirstBoard,
                   (byte)P1240.X_Axis,
                   8);
            //Y1 - 56 (Z thuc)
            retMsg = P1240.P1240MotDO(
                    Global.gFirstBoard,
                    (byte)P1240.Y_Axis,
                    6);


            //Set truc Z1 (Y thuc)
            retMsg = P1240.P1240MotDO(
                   Global.gFirstBoard,
                   (byte)P1240.Z_Axis,
                   8);
            //U1 - Y thuc
            retMsg = P1240.P1240MotDO(
                 Global.gFirstBoard,
                 (byte)P1240.U_Axis,
                 6);
        }
        private void ServoOn2ndCard()
        {
            int retMsg;
            //Set emg, stroke, servoOn 
            //Setting for 1st
            //Set truc X1(Z thuc)
            retMsg = P1240.P1240MotDO(
                   Global.gSecondBoard,
                   (byte)P1240.Z_Axis,
                   8);
            //Y1 - 56 (Z thuc)
            retMsg = P1240.P1240MotDO(
                    Global.gSecondBoard,
                    (byte)P1240.U_Axis,
                    6);
        }

        public void SetEncoder()
        {
            int reMsg = P1240.P1240SetEncoderMultiple(
                        Global.gFirstBoard, //Board ID number 
                        P1240.XYZU_Axis,  //Assign the Axis 
                        0      //multiple
                        );
            reMsg = P1240.P1240SetZLogic(
                    Global.gFirstBoard, //Board ID number 
                    P1240.XYZU_Axis,  //Assign the Axis 
                    0    //logic level
                    );
        }

        public void SetupServoParameter()
        {
            int errCode;

            errCode = P1240.P1240SetAcceleration(Global.gFirstBoard, (byte)P1240.Y_Axis, 200000000);
            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return;
            }

            errCode = P1240.P1240SetStartSpeed(Global.gFirstBoard, (byte)P1240.Y_Axis, 5000);
            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return;
            }

            errCode = P1240.P1240SetDrivingSpeed(Global.gFirstBoard, (byte)P1240.Y_Axis, 200000);
            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return;
            }

            errCode = P1240.P1240SetAcceleration(Global.gFirstBoard, (byte)P1240.U_Axis, 200000000);
            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return;
            }

            errCode = P1240.P1240SetStartSpeed(Global.gFirstBoard, (byte)P1240.U_Axis, 5000);
            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return;
            }

            errCode = P1240.P1240SetDrivingSpeed(Global.gFirstBoard, (byte)P1240.U_Axis, 200000);
            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return;
            }
            errCode = P1240.P1240SetAcceleration(Global.gSecondBoard, (byte)P1240.U_Axis, 400000000);
            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return;
            }

            errCode = P1240.P1240SetStartSpeed(Global.gSecondBoard, (byte)P1240.U_Axis, 5000);
            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return;
            }

            errCode = P1240.P1240SetDrivingSpeed(Global.gSecondBoard, (byte)P1240.U_Axis, 8000);
            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return;
            }

        }

        public void StopMode()
        {
            int errCode;

            // Stop continue drive
            errCode = P1240.P1240MotStop(
                Global.gFirstBoard,        // Board ID
                (byte)P1240.XYZU_Axis,           // Which axis to stop
                1);                  // Buffer ID
            errCode = P1240.P1240MotStop(
                Global.gSecondBoard,        // Board ID
                (byte)P1240.XYZU_Axis,           // Which axis to stop
                1);
            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return;
            }
        }

        public void Stop()
        {
            int retMsg;
            retMsg = P1240.P1240MotDO(
                   Global.gFirstBoard,
                   (byte)P1240.YU_Axis,
                   8);
            retMsg = P1240.P1240MotDO(
                   Global.gSecondBoard,
                   (byte)P1240.U_Axis,
                   8);
            retMsg = P1240.P1240MotDO(
                  Global.gFirstBoard,
                  (byte)P1240.YU_Axis,
                  9);
            retMsg = P1240.P1240MotDO(
                   Global.gSecondBoard,
                   (byte)P1240.U_Axis,
                   9);
        }

        public void ServoOnWithClear()
        {
            int retMsg;
            //Set emg, stroke, servoOn 

            //Set Card 1             
            //Set truc X1(Z thuc)
            retMsg = P1240.P1240MotDO(
                   Global.gFirstBoard,
                   (byte)P1240.X_Axis,
                   9);
            //Y1
            retMsg = P1240.P1240MotDO(
                    Global.gFirstBoard,
                    (byte)P1240.Y_Axis,
                    9);

            //Set truc Z1 (Y thuc)
            retMsg = P1240.P1240MotDO(
                   Global.gFirstBoard,
                   (byte)P1240.Z_Axis,
                   8);
            //U1 - Y thuc
            retMsg = P1240.P1240MotDO(
                Global.gFirstBoard,
                (byte)P1240.U_Axis,
                9);

            //Set Card 2 
            //Set truc U2 (X thuc)
            retMsg = P1240.P1240MotDO(
                   Global.gSecondBoard,
                   (byte)P1240.Z_Axis,
                   8);
            retMsg = P1240.P1240MotDO(
                Global.gSecondBoard,
                (byte)P1240.U_Axis,
                9);
        }


        public void CloseServo()
        {

        }
        public void StopSource()// ngat nguon
        {
            int retMsg = P1240.P1240MotDO(
                        Global.gFirstBoard,
                        (byte)P1240.X_Axis,
                        0);
        }

        private void SetSpeedStep()
        {
            int errCode = P1240.P1240SetDrivingSpeed(Global.gFirstBoard, (byte)P1240.X_Axis, 1000);
            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return;
            }

            errCode = P1240.P1240SetDrivingSpeed(Global.gFirstBoard, (byte)P1240.Z_Axis, 1000);
            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return;
            }


            errCode = P1240.P1240SetDrivingSpeed(Global.gSecondBoard, (byte)P1240.X_Axis, 1000);
            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return;
            }
            errCode = P1240.P1240SetDrivingSpeed(Global.gSecondBoard, (byte)P1240.Y_Axis, 1000);
            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return;
            }
            errCode = P1240.P1240SetDrivingSpeed(Global.gSecondBoard, (byte)P1240.Z_Axis, 1000);
            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return;
            }

        }
        /// <summary>
        /// THIS FUNCTION IS NO LONGER RUN IN PULSE VALUE. 
        /// Input position.Add convert func. before run.
        /// </summary>
        /// <param name="board"></param>
        /// <param name="lPulseNum"></param>
        /// <param name="axis"></param>
        /// <param name="absolute"></param>
        public void RunMotor(byte board, double position, byte axis, bool absolute = false)
        {
            int lPulseNum = (int)Math.Round(GetRealPulseFromPosition(board, axis) * position);

            int errCode;
            if (absolute)
                errCode = P1240.P1240MoveAbs(board, axis, lPulseNum);
            else
                errCode = P1240.P1240MoveIns(board, axis, lPulseNum);
            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return;
            }
        }
        /// <summary>
        /// Servo Encoder: 262144 pulse/r
        /// </summary>
        /// <param name="board"></param>
        /// <param name="axis"></param>
        /// <returns></returns>
        double GetRealPulseFromPosition(byte board, byte axis)
        {
            if (board == 9)
            {
                switch (axis)
                {
                    case 1:
                        return 1;//1000pulse/ rev. Dont' know 1 resolution = ?mm
                        break;
                    case 2:
                        return 262144 / 5000/4;//driver servo x 4
                        break;

                    case 4:
                        return 1;
                        break;

                    case 8:
                        return 262144 / 5000/4;//driver servo x 4
                        break;
                    default:
                        return 0;
                }
            }
            if (board == 10)
            {
                switch (axis)
                {
                    case 1:
                        return 1;
                        break;

                    case 2:
                        return 1;
                        break;

                    case 4:
                        return 1;
                        break;

                    case 8:
                        return 262144 / 5000 /8/4;//Driver servo da x8 san roi
                        break;
                    default:
                        return 0;
                }
            }
            return 0;

        }


        public void GotoXY(double posx,double posy)
        { 
            RunMotorStep(Global.gFirstBoard,posx, P1240.Y_Axis);
            RunMotorStep(Global.gFirstBoard,posy, P1240.U_Axis);
        }

        private void RunMotorStep(byte board, double position, byte axis, bool absolute = false)
        {
            int errCode;
            int lPulseNum = (int)Math.Round(GetRealPulseFromPosition(board, axis) * position);
            
            errCode = P1240.P1240SetDrivingSpeed(board, (byte)axis,1000);
            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return;
            }

            if (absolute)
                errCode = P1240.P1240MoveAbs(board, axis, lPulseNum);
            else
                errCode = P1240.P1240MoveIns(board, axis, lPulseNum);
            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return;
            }

        }
        public void ControlKnife(double pos)
        {
            RunMotorStep(Global.gFirstBoard, pos, P1240.X_Axis);
        }

        public void ReadRealEncoder(uint lFXPos, uint lFYPos,uint lFZPos,uint lFUPos,uint lSXPos,uint lSYPos,uint lSZPos,uint lSUPos)
        {
            int errCode;

            uint lFXPosi = 0;
            uint lFYPosi = 0;
            uint lFZPosi = 0;
            uint lFUPosi = 0;
            uint lSXPosi = 0;
            uint lSYPosi = 0;
            uint lSZPosi = 0;
            uint lSUPosi = 0;

            // Read P1240 4 axes logical posiition
            errCode = P1240.P1240MotRdMutiReg(
                Global.gFirstBoard,  // Board ID
                (byte)P1240.XYZU_Axis,      // Which axes
                (ushort)P1240.Rcnt,           // Logical counter
                ref lFXPosi,        // X axis position
                ref lFYPosi,        // Y axis position
                ref lFZPosi,        // Z axis position
                ref lFUPosi);       // U axis position
            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return;
            }

            errCode = P1240.P1240MotRdMutiReg(
                Global.gSecondBoard,  // Board ID
                (byte)P1240.XYZU_Axis,      // Which axes
                (ushort)P1240.Rcnt,           // Logical counter
                ref lSXPosi,        // X axis position
                ref lSYPosi,        // Y axis position
                ref lSZPosi,        // Z axis position
                ref lSUPosi);       // U axis position
            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return;
            }
            // Show value of encoder
            lFXPos = lFXPosi;
            lFYPos = lFYPosi;
            lFZPos = lFZPosi;
            lFUPos = lFUPosi;

            lSXPos = lSXPosi;
            lSYPos = lSYPosi;
            lSZPos = lSZPosi;
            lSUPos = lSUPosi;   
        }

        public void ReadLogicalEncoder(uint lFXPos, uint lFYPos, uint lFZPos, uint lFUPos, uint lSXPos, uint lSYPos, uint lSZPos, uint lSUPos)
        {
            int errCode;

            uint lFXPosi = 0;
            uint lFYPosi = 0;
            uint lFZPosi = 0;
            uint lFUPosi = 0;
            uint lSXPosi = 0;
            uint lSYPosi = 0;
            uint lSZPosi = 0;
            uint lSUPosi = 0;

            // Read P1240 4 axes logical posiition
            errCode = P1240.P1240MotRdMutiReg(
                Global.gFirstBoard,  // Board ID
                (byte)P1240.XYZU_Axis,      // Which axes
                (ushort)P1240.Lcnt,           // Logical counter
                ref lFXPosi,        // X axis position
                ref lFYPosi,        // Y axis position
                ref lFZPosi,        // Z axis position
                ref lFUPosi);       // U axis position
            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return;
            }

            errCode = P1240.P1240MotRdMutiReg(
                Global.gSecondBoard,  // Board ID
                (byte)P1240.XYZU_Axis,      // Which axes
                (ushort)P1240.Lcnt,           // Logical counter
                ref lSXPosi,        // X axis position
                ref lSYPosi,        // Y axis position
                ref lSZPosi,        // Z axis position
                ref lSUPosi);       // U axis position
            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return;
            }
            // Show value of encoder
            lFXPos = lFXPosi;
            lFYPos = lFYPosi;
            lFZPos = lFZPosi;
            lFUPos = lFUPosi;

            lSXPos = lSXPosi;
            lSYPos = lSYPosi;
            lSZPos = lSZPosi;
            lSUPos = lSUPosi;
        }


        public void Limit(ref bool bRoller,ref bool bX,ref bool bY,ref bool bZ,ref bool bKnife)
        {
            uint errlmt = 0;
            uint errlmtRoller = 0;
            uint errlmtX = 0;
            uint errlmtKnife = 0;
            uint errlmtY = 0;
            
            //Read Error First
            int errCode = P1240.P1240MotRdReg(
                Global.gFirstBoard,  // Board ID
                (byte)P1240.XYZU_Axis,      // Which axes
                (ushort)P1240.RR0,           // Logical counter
                ref errlmt
                );
            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return;
            }
            uint err = errlmt >> 4;
            if (err <= 0) return;

            //Read error for every axis
            //Roller 16 ; X 32 ; Knife 64 ; Y 128
            
            errCode = P1240.P1240MotRdMutiReg(
                Global.gFirstBoard,  // Board ID
                (byte)P1240.XYZU_Axis,      // Which axes
                (ushort)P1240.RR2,           // Logical counter
                ref errlmtRoller,
                ref errlmtX,
                ref errlmtKnife,
                ref errlmtY
                );

            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return ;
            }
            //Read Error First
            errCode = P1240.P1240MotRdReg(
                Global.gSecondBoard,  // Board ID
                (byte)P1240.XYZU_Axis,      // Which axes
                (ushort)P1240.RR0,           // Logical counter
                ref errlmt
                );
            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return;
            }
            
            err = errlmt >> 4;
            if (err < 0) return;

            //Read error for every axis
            //Roller 16 ; X 32 ; Knife 64 ; Y 128
            
            errCode = P1240.P1240MotRdMutiReg(
                Global.gSecondBoard,  // Board ID
                (byte)P1240.XYZU_Axis,      // Which axes
                (ushort)P1240.RR2,           // Logical counter
                ref errlmtRoller,
                ref errlmtX,
                ref errlmtKnife,
                ref errlmtY
                );

            if (errCode != 0)
            {
                Global.ShowError(errCode);
                return ;
            }


            return ; 

        }
        public bool busy()
        {
            int b1 = P1240.P1240MotAxisBusy(
                Global.gFirstBoard,  // Board ID
                (byte)P1240.YU_Axis
                );
            int b2 = P1240.P1240MotAxisBusy(
                Global.gSecondBoard,  // Board ID
                (byte)P1240.U_Axis
                );
            if (b1 != 0 && b2 != 0 )
                return true;
            else return false;
        }
    }
}
 